{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "15a9ee5d",
      "metadata": {},
      "source": [
        "# NavStateImuEKF\n",
        "\n",
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/NavStateImuEKF.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
        "\n",
        "## Overview\n",
        "\n",
        "The NavStateImuEKF is a left-invariant Lie group EKF for GTSAM’s NavState X = (R, p, v). It integrates IMU to predict motion and supports generic measurements (e.g., position). Increments for p and v are in the body frame, consistent with GTSAM’s conventions.\n",
        "\n",
        "This user guide covers:\n",
        "- The EKF state and local coordinates [δθ, δp_body, δv_body].\n",
        "- Predict from IMU and covariance propagation at each step.\n",
        "- Adding a world-position measurement with the correct Jacobian H = [0, R, 0].\n",
        "- Visualizing results with ±2σ uncertainty bands and basic tuning.\n",
        "\n",
        "See also: [NavState IMU EKF Tutorial](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/NavStateImuExample.ipynb) for a longer, step-by-step walkthrough."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "9258793f",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "# Install GTSAM and Plotly from pip if running in Google Colab\n",
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop \n",
        "except ImportError:\n",
        "    pass # Not in Colab"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "73dfedda",
      "metadata": {},
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "\n",
        "import gtsam\n",
        "from gtsam import NavState, Rot3, Pose3"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "cd8aa9c8",
      "metadata": {},
      "source": [
        "## State and local coordinates\n",
        "\n",
        "- State: X = (R, p, v) with rotation R ∈ SO(3), position p ∈ R³, velocity v ∈ R³.\n",
        "- Local coordinates: [δθ, δp_body, δv_body]. Increments for position/velocity are in the body frame.\n",
        "- Position measurement (world): z ≈ p_world has Jacobian H = [0, R, 0] in these local coordinates."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "e82f91b3",
      "metadata": {},
      "source": [
        "## API overview\n",
        "\n",
        "Class: `gtsam.NavStateImuEKF(X0, P0, params)`\n",
        "- Constructor: initial state `X0: NavState`, covariance `P0: 9x9`, and `PreintegrationParams` (gravity and IMU covariances).\n",
        "- Accessors: `state() -> NavState`, `covariance() -> 9x9 numpy array`.\n",
        "- Predict: `predict(omega, accel, dt)` integrates IMU over dt and propagates covariance.\n",
        "- Update (Python): `updateWithVector(prediction, H, measurement, R)` applies a linear update with measurement vector and Jacobian H in EKF local coordinates.\n",
        "  - Use this to mimic measurement-function-based updates by building H and z for your sensor."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "dbc4dd82",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initialized EKF: R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "p: 0 0 0\n",
            "v: 0 0 0\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Example of modeling an IMU using NED coordinates as the navigation frame \n",
        "# with sensor measurements in the body/sensor frame\n",
        "params = gtsam.PreintegrationParams.MakeSharedD(9.81)  # gravity (m/s^2)\n",
        "params.setAccelerometerCovariance(np.diag([1e-3, 1e-3, 1e-3]))\n",
        "params.setIntegrationCovariance(np.diag([1e-3, 1e-3, 1e-3]))\n",
        "params.setGyroscopeCovariance(np.diag([1e-4, 1e-4, 1e-4]))\n",
        "\n",
        "# Initial state and covariance, FRD frame aligned with NED (i.e., looking north)\n",
        "X0 = NavState(Pose3(), np.zeros(3))\n",
        "P0 = np.eye(9) * 0.1\n",
        "\n",
        "ekf = gtsam.NavStateImuEKF(X0, P0, params)\n",
        "print(\"Initialized EKF:\", ekf.state())"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "eedaaab8",
      "metadata": {},
      "source": [
        "## Predict: integrate IMU\n",
        "\n",
        "Signature: `predict(omega, accel, dt)`\n",
        "- `omega`: body angular velocity (rad/s), shape (3,).\n",
        "- `accel`: specific force (m/s²), shape (3,), in the body frame.\n",
        "- `dt`: timestep (s).\n",
        "\n",
        "Effect: builds a NavState increment from (omega, accel, dt), composes it onto the state, and updates covariance using a linearized transition. Process noise is scaled by dt inside the filter."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "6c519f19",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "After predict, state: R: [\n",
            "\t1, 0.001, 0;\n",
            "\t-0.001, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "p: 0 0 0\n",
            "v: 0 0 0\n",
            "\n",
            "Covariance diag: [0.31622935 0.31622935 0.31622935 0.31625943 0.31625943 0.31625939\n",
            " 0.31776148 0.31776148 0.31624358]\n"
          ]
        }
      ],
      "source": [
        "# Example predict with constant yaw rate and gravity-only accel\n",
        "omega_b = np.array([0.0, 0.0, -0.1])  # rad/s\n",
        "f_b = np.array([0.0, 0.0, -9.81])  # m/s^2 (static)\n",
        "dt = 0.01  # s\n",
        "ekf.predict(omega_b, f_b, dt)\n",
        "print(\"After predict, state:\", ekf.state())\n",
        "print(\"Covariance diag:\", np.sqrt(np.diag(ekf.covariance())))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "94afe951",
      "metadata": {},
      "source": [
        "## Update: using `updateWithVector` (Python)\n",
        "\n",
        "Signature: `updateWithVector(prediction, H, measurement, R)`\n",
        "- `prediction`: the measurement predicted by the current state (e.g., current world position p).\n",
        "- `H`: measurement Jacobian w.r.t. EKF local coordinates [δθ, δp_body, δv_body].\n",
        "- `measurement`: the actual measurement vector (same shape as prediction).\n",
        "- `R`: measurement covariance matrix.\n",
        "\n",
        "Position example (world position z ≈ p_world):\n",
        "- Prediction: `p_pred = ekf.state().position()`\n",
        "- Jacobian: `H = [0, R, 0]` where `R = ekf.state().attitude().matrix()` and H has shape (3,9)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "id": "f8ac9b74",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "After position update, state: R: [\n",
            "\t0.999999, 0.001, -8.17473e-06;\n",
            "\t-0.001, 0.999999, 4.08736e-06;\n",
            "\t8.17881e-06, -4.07919e-06, 1\n",
            "]\n",
            "p:   0.0166694 -0.00833472           0\n",
            "v:  0.000167463 -8.37315e-05            0\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# One-shot world-position update example\n",
        "X_pred = ekf.state()\n",
        "p_pred = X_pred.position()  # shape (3,)\n",
        "R_state = X_pred.attitude().matrix()  # 3x3\n",
        "H = np.zeros((3, 9))\n",
        "H[:, 3:6] = R_state\n",
        "\n",
        "# Suppose we measured a world position (e.g., GPS)\n",
        "z = p_pred + np.array([0.1, -0.05, 0.0])  # fake measurement a bit away\n",
        "R_meas = np.eye(3) * 0.5  # covariance (m^2)\n",
        "\n",
        "ekf.updateWithVector(p_pred, H, z, R_meas)\n",
        "print(\"After position update, state:\", ekf.state())"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "bfa88ba4",
      "metadata": {},
      "source": [
        "## Full examples and plotting\n",
        "\n",
        "For a step-by-step tutorial with scenarios, plots, and uncertainty bands, see:\n",
        "- NavState IMU EKF Tutorial: https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/NavStateImuExample.ipynb"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "5309dea2",
      "metadata": {},
      "source": [
        "## Tips for other sensors\n",
        "\n",
        "- Any measurement can be used by providing its prediction and Jacobian H in the EKF’s local coordinates.\n",
        "- Examples:\n",
        "  - Body-frame velocity: H typically selects the δv_body block.\n",
        "  - Orientation (e.g., magnetometer yaw): derive H w.r.t. δθ.\n",
        "- Always ensure units/frames are consistent (world vs. body) when building prediction and H."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "f01c7923",
      "metadata": {},
      "source": [
        "## Source\n",
        "\n",
        "- [NavStateImuEKF.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/NavStateImuEKF.h)\n",
        "\n",
        "- [NavStateImuEKF.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/NavStateImuEKF.cpp)\n",
        "\n",
        "- [LeftLinearEKF.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/LeftLinearEKF.h)\n",
        "\n",
        "- [ManifoldEKF.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ManifoldEKF.h)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}